#include <vector>
#include <stdio.h>
#include "Math/Math3d.h"
#include "Math/Vecd.h"
#include "Math/Matd.h"

typedef unsigned int uint;
typedef unsigned char uchar;

//A1
#define BIN_THRES 0

//the input is probably a CByteImage, which already has width/height
//the pixels are still a 1D array, though
std::vector<double> getLaserLine(uchar* img, uint width, uint height) {
  std::vector<double> pList(height);

  for (uint y = 0; y < height; y++) {
    double lineX = 0;
    bool foundStart = false;

    double intensitySum = 0;
    double weightedSum = 0; //weight = intensity value

    for (uint x = 0; x < width; x++) {\
      uchar imgVal = img[y * width + x];
      //printf("(%d,%d): img: %d, found: %d\n", y, x, imgVal, foundStart);
      if (!foundStart && imgVal >= BIN_THRES) {
        foundStart = true;
      }
      if (foundStart) {
        if (imgVal < BIN_THRES) break; //line ended

        intensitySum += imgVal;
        weightedSum += (x + 1) * imgVal; //+1 so that pixels start w/ 1
        //printf("(%d,%d):%f and %f\n", y, x, intensitySum, weightedSum);
      }
    }
    lineX = (foundStart)? weightedSum / intensitySum - 1 : -1;
    pList[y] = lineX;
  }

  return pList;
}

//A3

//TODO: I just noticed that there's a CVec, which doesn't use the obnoxious
//      C-style methods, so this should probably be redone?

//offset of plane, normal of plane, offset of line, direction of line
//if n is not given, use crossP of both plane vectors
Vec3d getIntersection(Vec3d a_p, Vec3d n_p, Vec3d a_l, Vec3d d_l) {
  Vec3d result;
  Vec3d temp; //used as storage for multiplication of scalar with vector

  //calc scalar of line intersecting the plane
  double r = (- Math3d::ScalarProduct(a_l,n_p) + Math3d::ScalarProduct(a_p,n_p))
            / Math3d::ScalarProduct(d_l,n_p);

  //use scalar to calc the actual intersection point
  Math3d::MulVecScalar(d_l, r, temp);
  Math3d::AddVecVec(a_l,  temp, result);

  return result;
}

//A2
CVecd leastSquares(CMatd A, CVecd b) {
  CMatd A_t = A.GetTransposed();

  CVecd result = (A_t * A).Invert() * A_t * b;

  return result;
}

CVecd leastSquaresFromList(std::vector<CVecd> vList) {
  CMatd A(vList.size(), vList[0].GetSize());
  CVecd b(vList.size());

  for (uint r = 0; r < A.GetRows(); r++) {
    b[r] = vList[r][0];
    A(r, 0) = 1;
    for (uint c = 1; c < A.GetColumns(); c++) {
      A(r, c) = vList[r][c];
    }
  }

  return leastSquares(A, b);
}

int main() {
  //A1 test
  uint w = 4;
  uint h = 3;
  uchar img[] = {
    0, 200, 200, 0,
    200, 250, 200, 0,
    100, 255, 100, 50
  };

  std::vector<double> pts = getLaserLine(img, w, h);

  for (uint i = 0; i < pts.size(); i++) {
    printf("p%d: (x,y)=(%.2f,%.2f)\n", i, pts[i], (double)i);
  }

  //A2 test
  Vec3d n_p, a_p, d_l, a_l;
  Math3d::SetVec(n_p, 0, 1, 0);
  Math3d::SetVec(a_p, 1, 1, 1);
  Math3d::SetVec(d_l, 1, 1, 0);
  Math3d::SetVec(a_l, 0, -1, 0);
  Vec3d res = getIntersection(a_p, n_p, a_l, d_l);
  printf("Vec: (%.2f,%.2f,%.2f)\n", res.x, res.y, res.z);
  return 0;
}
